#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/ros.h>
#include <ros/console.h>

#include <iostream>
#include <fstream>
#include <string>

int ekfCounter = 0;
int encoderCounter = 0;

void GpsCallback(const nav_msgs::Odometry &msg) {
  std::ofstream myfile;
  myfile.open("/home/sumo/sumo/su-sumo/pose_ekf_simulation/output/gps.txt", std::ios::app);
  double secs = ros::Time::now().toSec();
  if (!myfile.is_open()) {
    ROS_ERROR("GPS FILE NOT OPEN!");
  }
  myfile << secs << " " 
	 << msg.pose.pose.position.x << " "
	 << msg.pose.pose.position.y << " "
	 << "\n";
  
  myfile.close();
}

void ImuCallback(const sensor_msgs::Imu &msg) {
    std::ofstream myfile;
  myfile.open("/home/sumo/sumo/su-sumo/pose_ekf_simulation/output/encoder.txt", std::ios::app);
  double secs =ros::Time::now().toSec();
  myfile << secs << " " 
	 << msg.pose.pose.position.x << " "
	 << msg.pose.pose.position.y << " "
	 << "\n";
  encoderCounter = 0;
  myfile.close();
}

void WheelCallback(const nav_msgs::Odometry &msg) {
  encoderCounter++;
  std::ofstream myfile;
  myfile.open("/home/sumo/sumo/su-sumo/pose_ekf_simulation/output/encoder.txt", std::ios::app);
  double secs =ros::Time::now().toSec();
  if (encoderCounter % 10 == 0) {

    myfile << secs << " " 
	   << msg.pose.pose.position.x << " "
	   << msg.pose.pose.position.y << " "
	   << "\n";
    encoderCounter = 0;
  }
  myfile.close();
}

void EkfCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
  ekfCounter++;
  std::ofstream myfile;
  myfile.open("/home/sumo/sumo/su-sumo/pose_ekf_simulation/output/ekf.txt", std::ios::app);
  double secs =ros::Time::now().toSec();
  if (!myfile.is_open()) {
    ROS_ERROR("EKF FILE NOT OPEN!");
  }
  if (ekfCounter % 10 == 1) {
    myfile << secs << " " 
	   << msg->pose.pose.position.x << " "
	   << msg->pose.pose.position.y << " "
	   << "\n";
    ekfCounter = 0;
  }
myfile.close();
}

int main(int argc, char** argv) {
  ros::Subscriber gps_sub;
  ros::Subscriber imu_sub;
  ros::Subscriber wheel_sub;
  ros::Subscriber pose_sub;

  ros::init(argc, argv, "pose_ekf_sim");
  ros::NodeHandle n;

  gps_sub = n.subscribe("vo", 500, &GpsCallback);
  imu_sub = n.subscribe("imu_data", 10, &ImuCallback);
  wheel_sub = n.subscribe("odom", 10, &WheelCallback);
  pose_sub = n.subscribe("pose_ekf/odom_combined", 10, &EkfCallback);

  ros::spin();
  return 0;
}
